﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

#include <learn/geom/quat.h>

using namespace xi;
using namespace xi::geom;

TEST(TestQuat, basic)
{
    {
        Quat<double> qa(2.0, Vec3{-2.0, 3.0, -4.0});
        Quat<double> qb(1.0, Vec3{-2.0, 5.0, -6.0});
        EXPECT_EQ(qa + qb, Quat<double>(3.0, Vec3{-4.0, 8.0, -10.0}));
        EXPECT_EQ(qa - qb, Quat<double>(1.0, Vec3{0.0, -2.0, 2.0}));
        EXPECT_EQ(qa * 2, Quat<double>(4.0, Vec3{-4.0, 6.0, -8.0}));
        EXPECT_EQ(qa / 2, Quat<double>(1.0, Vec3{-1.0, 1.5, -2.0}));
        EXPECT_FLOAT_EQ(qa.modulus(), sqrt(33));
        EXPECT_FLOAT_EQ(qb.modulus(), sqrt(66));
        EXPECT_EQ(qa.normalized(), (qa / sqrt(33)));
        EXPECT_EQ(qb.normalized(), (qb / sqrt(66)));
        EXPECT_EQ(qa * qb, Quat<double>(-41.0, Vec3{-4.0, 9.0, -20.0}));
        EXPECT_EQ(qb * qa, Quat<double>(-41.0, Vec3{-8.0, 17.0, -12.0}));
        EXPECT_EQ((Quat<double>(1.0, Vec3{0.0, 0.0, 0.0}) / qa), (Quat<double>(2.0, Vec3{2.0, -3.0, 4.0}) / 33));
    }

    {
        Quat<double> qa(2.0, Vec3{-2.0, 3.0, -4.0});
        Quat<double> qb(1.0, Vec3{-2.0, 5.0, -6.0});
        qa += qb;
        EXPECT_EQ(qa, Quat<double>(3.0, Vec3{-4.0, 8.0, -10.0}));

        qa -= qb * 2;
        EXPECT_EQ(qa, Quat<double>(1.0, Vec3{0.0, -2.0, 2.0}));

        qa += qb;
        qa *= 2;
        EXPECT_EQ(qa, Quat<double>(4.0, Vec3{-4.0, 6.0, -8.0}));

        qa /= 4;
        EXPECT_EQ(qa, Quat<double>(1.0, Vec3{-1.0, 1.5, -2.0}));

        qa *= qb * 2;
        EXPECT_EQ(qa, Quat<double>(-41.0, Vec3{-4.0, 9.0, -20.0}));

        qa /= qb * 2;
        EXPECT_EQ(qa, Quat<double>(1.0, Vec3{-1.0, 1.5, -2.0}));
    }
}

TEST(TestQuat, rotation)
{

    {
        Vec3 v(1.0 / sqrt(2), 0.0, 1.0 / sqrt(2));
        Quat<double> q = Quat<double>::rotate(v, math::Numeric::pi / 2);
        Quat<double> p(0.0, Vec3{2.0, 0.0, 0.0});

        // 逆时针绕 v 转 90 度
        auto pp = q * p / q;
        EXPECT_LE((pp - Quat<double>(0.0, Vec3(1.0, sqrt(2), 1.0))).modulus(), 1e-6);

        // 从一个轴转到另一个轴
        Vec3 v1(2.4, 3.6, 5.8);
        v1.normalize();
        Vec3 v2(5.1, 9.6, -2.7);
        v2.normalize();
        Quat<double> q2 = Quat<double>::rotate(v1, v2);
        EXPECT_LE(((q2 * Quat<double>(0.0, v1) / q2) - Quat<double>(0.0, v2)).modulus(), 1e-6);
    }

    // 当转角 theta / 2 小于 45 度时，矩阵迹 > 0
    {
        Vec3 v(1.0 / sqrt(2), 0.0, 1.0 / sqrt(2));
        Quat<double> q = Quat<double>::rotate(v, math::Numeric::pi / 3);
        auto m = Quat<double>::to_mat4x4(q);
        auto q1 = Quat<double>::from_mat4x4(m);

        Vec3 o(1.0, 2.0, 3.0);
        auto p0 = m * cartesian_to_homogenous(o);
        Quat q0(0.0, o);
        EXPECT_LE((q * q0 / q - q1 * q0 / q1).modulus(), 1e-6);
    }

    // 当转角 theta / 2 大于 45 度时，矩阵迹 < 0
    // 分别测试最大元素位于 m(0, 0), m(1, 1), m(2, 2) 三个位置
    {
        Vec3 v(1.0, 0.0, 0.0);
        Quat<double> q = Quat<double>::rotate(v, math::Numeric::pi / 1.5);
        auto m = Quat<double>::to_mat4x4(q);
        auto q1 = Quat<double>::from_mat4x4(m);

        Vec3 o(1.0, 2.0, 3.0);
        auto p0 = m * cartesian_to_homogenous(o);
        Quat q0(0.0, o);
        EXPECT_LE((q * q0 / q - q1 * q0 / q1).modulus(), 1e-6);
    }

    {
        Vec3 v(0.0, 1.0, 0.0);
        Quat<double> q = Quat<double>::rotate(v, math::Numeric::pi / 1.5);
        auto m = Quat<double>::to_mat4x4(q);
        auto q1 = Quat<double>::from_mat4x4(m);

        Vec3 o(1.0, 2.0, 3.0);
        auto p0 = m * cartesian_to_homogenous(o);
        Quat q0(0.0, o);
        EXPECT_LE((q * q0 / q - q1 * q0 / q1).modulus(), 1e-6);
    }

    {
        Vec3 v(0.0, 0.0, 1.0);
        Quat<double> q = Quat<double>::rotate(v, math::Numeric::pi / 1.5);
        auto m = Quat<double>::to_mat4x4(q);
        auto q1 = Quat<double>::from_mat4x4(m);

        Vec3 o(1.0, 2.0, 3.0);
        auto p0 = m * cartesian_to_homogenous(o);
        Quat q0(0.0, o);
        EXPECT_LE((q * q0 / q - q1 * q0 / q1).modulus(), 1e-6);
    }
}